#pragma once 
#include <Eigen/Dense>
#include "S03.h"

#include <functional>
#include <iostream>
namespace SlamCraft
{
    class IMU
    {
    public:
        Eigen::Vector3d acc = Eigen::Vector3d::Zero();
        Eigen::Vector3d gro = Eigen::Vector3d::Zero();
        Eigen::Quaterniond rot = Eigen::Quaterniond::Identity();
        double stamp = 0.0;//ms
        void clear(){
            acc = Eigen::Vector3d::Zero();
            gro = Eigen::Vector3d::Zero();
            stamp = 0;
        }
        IMU operator + (const IMU& imu){
            IMU res;
            res.acc = this->acc+imu.acc;
            res.gro = this->gro+imu.gro;
            return res;
        }
        IMU operator *(double k){
            IMU res;
            res.acc = this->acc *k;
            res.gro = this->gro *k;
            res.stamp = this->stamp;
            return res;
        }
        IMU  operator/(double k){
            IMU res;
            res.acc = this->acc /k;
            res.gro = this->gro /k;
            res.stamp = this->stamp;
            return res;
        }
        friend std::ostream & operator<<(std::ostream& ostream,const IMU &imu){
            ostream<<"imu_time: "<<imu.stamp<<" ms | imu_acc: "<<imu.acc.transpose()<<" | imu_gro: "<< imu.gro.transpose() ;
            return ostream;
        }
    };
    class ESKF
    {
    public:
        struct State_18
        {
            Eigen::Quaterniond rotation;
            Eigen::Vector3d position;
            Eigen::Vector3d velocity;
            Eigen::Vector3d bg;
            Eigen::Vector3d ba;
            Eigen::Vector3d gravity;
            State_18(){
                rotation = Eigen::Quaterniond::Identity();
                position = Eigen::Vector3d::Zero();
                velocity = Eigen::Vector3d::Zero();
                bg = Eigen::Vector3d::Zero();
                ba = Eigen::Vector3d::Zero();
                gravity = Eigen::Vector3d::Zero();
            }
            friend std::ostream &operator<<( std::ostream &output, 
                                        const State_18 &state_18 )
            { 
                output<<"rotation: "<<state_18.rotation.coeffs().transpose()
                    <<"\npostion: "<<state_18.position.transpose()
                    <<"\nvelocity: "<<state_18.velocity.transpose()
                    <<"\nbg: "<<state_18.bg.transpose()
                    <<"\nba: "<<state_18.ba.transpose()
                    <<"\ngrav: "<<state_18.gravity.transpose()<<std::endl; ;
                return output;            
            }
        };
        std::function<void(const State_18& ,Eigen::MatrixXd & ,Eigen::MatrixXd & ,Eigen::MatrixXd & )> zhr_model;
        

    private:
        State_18 X,X_last;
        Eigen::Matrix<double,18,18> P;
        Eigen::Matrix<double,12,12> Q;
        int iter_times = 15;
        double abs_error_thre =  0.001;
        Eigen::Matrix3d  A_T(const Eigen::Vector3d& v);
    public:
        ESKF();
        ~ESKF();


        ///.s1-s2
        // todo  define operator
        Eigen::Matrix<double,18,1> getErrorState18(const State_18 &s1, const  State_18 &s2);
        void predict(const IMU & imu_data,double dt);
        void update_once();
        void update();
        void set_iter_times(int n){
            iter_times = n;
        }
        struct State_18 getX(){
            return X;
        }
        struct State_18 getX_last(){
            return X_last;
        }
        void setX(const struct State_18& in_x){
            X = in_x;
        }
        Eigen::Matrix<double,18,18> getP(){
            return P;
        }
        void setP(const Eigen::Matrix<double,18,18> &in_p){
            P = in_p;
        }
        Eigen::Matrix<double,12,12> getQ(){
            return Q;
        }
        void setQ(const Eigen::Matrix<double,12,12> &in_q){
            Q = in_q;
        }
    };


} 
